﻿using System;
using System.Collections.Generic;
using DavidJalbert.TinyCarControllerAdvance;
using UnityEngine;
using UnityEngine.Serialization;
using Yoozoo.Gameplay.Liberty.Media;
using Yoozoo.Gameplay.Liberty.VehicleController;

namespace Yoozoo.Gameplay.Liberty.AI
{
    [RequireComponent(typeof(VehicleSetting))]
    public class VehicleAI : MonoBehaviour
    {

        [Flags]
        public enum VehicleSpeedStrategy
        {
            None = 0,   // 外部控制
            Const = 1,  // 跟随路径时使用
            Curve = 2,  // 行进到目的地使用
            Follow = 4, // 追随目标
            Brake = 8,  // 刹车
            
            
            ImmobilizeBrake = 256,      // 到达目的地使用刚体冻结模式
            HandBrake = 512,            // 到达目的地使用手刹模式
            
            PreventBump = 4096,         // 防止撞击
        }
        
        public enum VehicleSteerStatus
        {
            Ahead = 1,      // 前进
            Reverse = 2,    // 倒车
        }

        public enum VehicleAIMode
        {
            Physics = 1,    // 物理驱动模式
            Spline = 2,     // 物理曲线跟随模式
            Curve = 3,      // 非物理曲线跟随模式
        }

        [Serializable]
        public class SplineModeData
        {
            [Header("加速力")]
            public float accForce = 0.65f;
            [Header("减速力")]
            public float decForce = 1;
            [Header("前方有阻挡时的减速距离")]
            public float slowDownDistance = 10;
            [Header("加速因子")]
            public float accFactor = 1;
            [Header("是否应用手刹")]
            public bool applyHandbrake;
            [Header("手刹力")]
            public float handBrakeForce = 2;
            [Header("转向速度")]
            public float turnSpeed = 1;

            [Header("轮子模拟转速")]
            public float wheelRotateSpeed = 5;
            [Header("转弯因子")]
            public float wheelSteeringFactor = 1;
        }

        [Serializable]
        public class CurveModeData
        {
            [Header("加速力")]
            public float accForce = 0.45f;
            [Header("减速力")]
            public float decForce = 1.65f;
            [Header("前方有阻挡时的减速距离")]
            public float slowDownDistance = 12;
            [Header("加速因子")]
            public float accFactor = 1;
            
            [Header("是否应用手刹")]
            public bool applyHandbrake;

            [Header("手刹减速因子")]
            public float handBrakeForce = 2.5f;
            [Header("转向速度")]
            public float turnSpeed = 1.2f;
            
            [Header("轮子模拟转速")]
            public float wheelRotateSpeed = 5;
            [Header("转弯因子")]
            public float wheelSteeringFactor = 1;
            
            public float followSlowDownFactor = 1f;
            
            // 当前速度
            public Vector3 velocity = Vector3.zero; 
        }
        
        [SerializeField]
        private bool active = true;

        public bool Active
        {
            get
            {
                return active;
            }
            set
            {
                if (_audioComponent!=null)
                {
                    if (value)
                    {
                        _audioComponent.loopAdjustToneSound.SetSoundValue(0);
                        _audioComponent.loopAdjustToneSound.enabled = true;
                        _audioComponent.loopAdjustToneSound.PlaySound(_audioComponent.engineSound);
                        
                        if(carBodyPhysic) carBodyPhysic.enabled = false;
                        if(carController) carController.enabled = false;
                    }
                    else
                    {
                        _audioComponent.loopAdjustToneSound.enabled = false;
                        _audioComponent.loopAdjustToneSound.StopSound(_audioComponent.engineSound);
                        if(carBodyPhysic) carBodyPhysic.enabled = true;
                        if(carController) carController.enabled = true;
                    }
                }
               
                active = value;
            }
        }

        
        public VehicleAIMode AIMode = VehicleAIMode.Physics;

        public SplineModeData splineModeData;

        public CurveModeData curveModeData;
        
        public VehicleSpeedStrategy speedStrategy = 
            VehicleSpeedStrategy.Const | VehicleSpeedStrategy.HandBrake;
        
        public VehicleSteerStatus steerStatus = VehicleSteerStatus.Ahead;
        
        [SerializeField]
        private float carBodyFVelocity;
        
        public float motorDeltaFactor = 1f;
        [SerializeField]
        private float motorDecelerateFactor = 1f;

        public float externalMotorFactor = 1f;
        // Const 策略下参数
        

        // Curve 策略下参数

        [Header("此范围车辆停止加速了，利用motorDrag进行减速")]
        [Range(1f,50f)][DisplayOnly]
        public float motor2ZeroRadius = 10f;
        [Range(0.1f,4f)]
        public float motorRadiusFactor = 2f;
        public AnimationCurve motorRadiusCurve = AnimationCurve.Linear(0, 1, 1, 1);
        // [Header("车辆进入Motor减速状态，此值越大，在motor减速半径越容易停下来")]
        // public AnimationCurve motorBrakeCurve = AnimationCurve.Linear(0,0,1,0.5f);
        // [Range(0.1f,100f)]
        // public float motorBrakeFactor = 1f;
        [Range(1, 50f)] 
        public float revertMotorRadius = 5f;
        [Range(0.1f,5f)]
        public float motorBrakeMinFv = 2f;  // 此范围允许减到的最小速度
        [Range(0.01f,2f)]
        public float reveseMotorAccDelta = 0.8f;
        
        [Header("距离目标越近，此值越小，车停下来越快")]
        public AnimationCurve radiusBrakeCurve = AnimationCurve.Linear(0,0,1,1);
        [Range(1f,5f)]
        public float radiusBrakeFactor = 1.5f;
        [Range(0.1f,10f)]
        public float arriveRadius = 3f;

        [SerializeField]
        private float radiusBrake = 1f;
        
        // Brake 策略下参数
        [Header("刹车速度")]
        public float brakeSpeed = 1f;
        
        //[SerializeField]
        public TCCAPlayer carController;
        
        [FormerlySerializedAs("carBody")] [SerializeField]
        public CarBodyPhysic carBodyPhysic; 
        
        [SerializeField]
        private PhysicsCasterCollection carCaster;
        private List<RaycastHit> _hitInfos = new List<RaycastHit>(4);

        [Header("需转向角度与steering值的比例")]
        public float steeringFactor = 0.01f;
        public float steeringAngleBound = 30f;
        public AnimationCurve steeringCurve = AnimationCurve.Linear(0,0,1,1);
        
        // 当前车辆的目标点
        [SerializeField]
        private Vector3 _destination;
        [SerializeField]
        private bool _terminal;
        
        [SerializeField] 
        private Vector3 _targetVector;
        
        [SerializeField]
        private float targetDistance;
        [SerializeField]
        private Transform debugTarget;

        private VehicleSetting _vehicleSetting;
        private VehicleAudioComponent _audioComponent;

        public int LOD { get; set; }
        
        private Transform SteerPoint
        {
            get
            {
                if (_vehicleSetting == null)
                {
                    if (carBodyPhysic == null)
                    {
                        return null;
                    }
                    return carBodyPhysic.transform;
                }
                
                if (steerStatus == VehicleSteerStatus.Ahead && _vehicleSetting.frontPoint)
                {
                    return _vehicleSetting.frontPoint;
                }
                else if (steerStatus == VehicleSteerStatus.Reverse && _vehicleSetting.backPoint)
                {
                    return _vehicleSetting.backPoint;
                }

                return carBodyPhysic.transform;
            }
        }


        public void Switch2SplineMode()
        {
            
        }
        
        public void SetDestination(Vector3 destination,bool terminal)
        {
            _destination = destination;
            _terminal = terminal;
        }

        public void SetMotorDelta(float motorDelta)
        {
            motorDeltaFactor = motorDelta;
        }

        public void SetBrakeSpeed(float s)
        {
            brakeSpeed = s;
        }

        public void SwitchSpeedStrategy(VehicleSpeedStrategy strategy)
        {
            speedStrategy = strategy;
        }

        private void Awake()
        {
            _vehicleSetting = GetComponent<VehicleSetting>();
            _audioComponent = GetComponent<VehicleAudioComponent>();
        }

        public void Init()
        {
            AIMode = VehicleAIMode.Curve; 
            //AIMode = VehicleAIMode.Spline; 
            carBodyPhysic = GetComponentInChildren<CarBodyPhysic>();
            // 根据车轮算下车身BoxCollider的size
            var bc = carBodyPhysic.boxCollider;
            if (bc)
            {

                var wheelRootPostion = carController.getWheelsRootPosition();
                float sizeY = Mathf.Abs((bc.transform.position + bc.center - wheelRootPostion).y);
                sizeY += carController.getWheelsMaxRadius();
                sizeY *= 2;
                bc.size = new Vector3(bc.size.x, sizeY, bc.size.z);
            }
            
            // 把车轮禁用
            // foreach (var wheel in carController.getWheels())
            // {
            //     wheel.wheelRootObject.gameObject.SetActive(false);
            //     wheel.enabled = false;
            // }
            
            carController.setWheelsEnabled(false);
            EnterCurveMode();
            //EnterSplineMode();
        }

        private void FixedUpdate()
        {
#if UNITY_EDITOR
            if (debugTarget!=null)
            {
                SetDestination(debugTarget.position, true);
            }

            targetDistance = Vector3.Distance(SteerPoint.position, _destination);
#endif
            
            if (!Active) return;

            if (AIMode == VehicleAIMode.Physics)
            {
                CalculateValues();
                Nav2Destination();
            }
            else if (AIMode == VehicleAIMode.Spline)
            {
                FollowPath();
            }
        }

        private void Update()
        {
            if (!Active) return;
            
            if (AIMode == VehicleAIMode.Curve)
            {
                FollowCurve(Mathf.Min(0.07f, Time.deltaTime));
            }
        }

        private int _castFrame = 0;
        private float tDistance = 9999;
        private float GetMinObstacleDistance()
        {
            // 前方碰撞检测 & 减速逻辑
            carCaster.transform.position = SteerPoint.position - SteerPoint.forward * 1.5f;
            carCaster.transform.rotation = SteerPoint.rotation;

            _castFrame++;

            int minCastFrame = AIMode == VehicleAIMode.Curve ? 6 : 4;  // 远处车辆检测精度降低
            
            if (_castFrame > minCastFrame)
            {
                float slowDownDistance = AIMode == VehicleAIMode.Curve
                    ? curveModeData.slowDownDistance
                    : splineModeData.slowDownDistance;
                
                carCaster.SetCastDistance(slowDownDistance + 1.5f);
                carCaster.Cast();
                _hitInfos.Clear();
                carCaster.GetHitInfo(ref _hitInfos);
                _castFrame = 0;
            }

            if (_hitInfos.Count > 0)
            {
                float minDis = 99999;
                foreach (var hitInfo in _hitInfos)
                {
                    if (hitInfo.collider == carController.CarCollider)
                    {
                        continue;
                    }
                        
                    float d = Vector3.Distance(SteerPoint.position, hitInfo.point);
                    if (d < minDis)
                    {
                        minDis = d;
                    }
                }

                tDistance = minDis;
            }
            else
            {
                tDistance = 99999;
            }

            return tDistance;
        }
        
        private float lastSteerAngle = 0;
        
        void FaceWaypoint(Vector3 relativePos, float turnSpeed, float wheelSteeringFactor)
        {
            Quaternion rotation = Quaternion.LookRotation(relativePos, Vector3.up);
            carBodyPhysic.transform.rotation = Quaternion.Slerp(carBodyPhysic.transform.rotation, rotation, Time.fixedDeltaTime * turnSpeed);  // slerp ( from.rotation, to.rotation, speed)

            if (LOD <= 1) // 远处车辆不模拟
            {
                // 模拟车轮转向
                relativePos.y = 0;
                Vector3 f = carBodyPhysic.transform.forward;
                f.y = 0;
                float angle = Vector3.Angle(relativePos.normalized, f);
                if (Vector3.Cross(relativePos.normalized, f).y > 0)
                {
                    angle *= -1;
                }

                angle *= wheelSteeringFactor * 0.5f;
                
                carController.SetSteeringAngle(angle,lastSteerAngle);
                
                lastSteerAngle = angle;
            }
        }

       

        //走到人边上
        public void EnterSplineMode()
        {
            AIMode = VehicleAIMode.Spline;
            var rb = carBodyPhysic.getRigidbody();
            rb.isKinematic = false;
            rb.collisionDetectionMode = CollisionDetectionMode.Continuous;
            rb.useGravity = true;
            rb.velocity = curveModeData.velocity;
            for (int i = 0; i < carCaster.casters.Count; i++)
            {
                if (carCaster.casters[i])
                {
                    carCaster.casters[i].enabled = true;
                }
            }
        }
        
        public void EnterCurveMode()
        {
            AIMode = VehicleAIMode.Curve;
            var rb = carBodyPhysic.getRigidbody();
            curveModeData.velocity = rb.velocity;
            rb.collisionDetectionMode = CollisionDetectionMode.ContinuousSpeculative;
            rb.isKinematic = true;
            rb.useGravity = true;
            for (int i = 0; i < carCaster.casters.Count; i++)
            {
                if (i%2 == 0) // 远处车辆射线检测次数减少
                {
                    if (carCaster.casters[i])
                    {
                        carCaster.casters[i].enabled = false;
                    }
                }
            }
        }
        
        #region CurveMode

        private void FollowCurve(float deltaTime)
        {
            Vector3 frontPos = SteerPoint.position;
            Vector3 tf = _destination - frontPos;

            GetMinObstacleDistance();
            if (curveModeData.applyHandbrake)
            {
                
                if (GetForwardVelocity(curveModeData.velocity) > 0)
                {
                    ChangeCurveModeVelocity(deltaTime, -curveModeData.velocity.normalized * curveModeData.handBrakeForce * 0.2f);
                }

                if (GetForwardVelocity(curveModeData.velocity) < 0 )
                {
                    curveModeData.velocity = Vector3.zero;
                }
            }
            else
            {
                bool brake = tDistance < curveModeData.slowDownDistance;
                if (!brake)
                {
                    if (GetForwardVelocity(curveModeData.velocity) < _vehicleSetting.aiMaxSpeed)
                    {
                        ChangeCurveModeVelocity(deltaTime, tf.normalized * (curveModeData.accForce * 0.2f * curveModeData.accFactor));
                    }
                    
                    Vector3 relativePos = _destination - carBodyPhysic.transform.position;
                    relativePos.y = 0;
                    FaceWaypoint(relativePos, curveModeData.turnSpeed, curveModeData.wheelSteeringFactor);
                }
                else
                {
                    if (GetForwardVelocity(curveModeData.velocity) > 0)
                    {
                        ChangeCurveModeVelocity(deltaTime, -curveModeData.velocity.normalized * curveModeData.decForce * 0.2f);
                    }

                    if (GetForwardVelocity(curveModeData.velocity) < 0)
                    {
                        curveModeData.velocity = Vector3.zero;
                    }
                }
            }

            curveModeData.velocity.y = 0;
            tf.y = 0;
            curveModeData.velocity =
                Vector3.Lerp(curveModeData.velocity.normalized, (_destination - carBodyPhysic.transform.position).normalized, deltaTime * 2) *
                curveModeData.velocity.magnitude;
            
            carBodyPhysic.getRigidbody().transform.position = (curveModeData.velocity * deltaTime + carBodyPhysic.transform.position);
            
            // 模拟车轮表现
            var rollSpeed = GetForwardVelocity(curveModeData.velocity) * curveModeData.wheelRotateSpeed;
            carController.RollWheels(rollSpeed);
           
            
            
            // 接音效
            float fv = carController.getSpeed() * 2.237f;
            float maxFv = _vehicleSetting.aiMaxSpeed;

            if (_audioComponent)
            {
                // 发动机音效
                float engineSoundValue = fv;
                // engineSoundValue *= 100;
                engineSoundValue = Mathf.Abs(engineSoundValue);
                _audioComponent.loopAdjustToneSound.SetSoundValue(engineSoundValue);
            }
        }

        private void ChangeCurveModeVelocity(float deltaTime, Vector3 v)
        {
            curveModeData.velocity += v * deltaTime / 0.034f;
        }

        private float GetForwardVelocity(Vector3 velocity)
        {
            return velocity.magnitude;
            return Vector3.Dot(velocity, carBodyPhysic.transform.forward);
        }
        
        #endregion

        #region SplineMode
        
        private void FollowPath()
        {
            Vector3 frontPos = SteerPoint.position;
            Vector3 tf = _destination - frontPos;

            GetMinObstacleDistance();
            
            if (splineModeData.applyHandbrake)
            {
                if (carBodyPhysic.getForwardVelocity() > 0)
                {
                    AddForce(-carBodyPhysic.transform.forward * splineModeData.handBrakeForce * 0.2f);
                }

                if (carBodyPhysic.getForwardVelocity() < 0)
                {
                    carBodyPhysic.getRigidbody().velocity = Vector3.zero;
                }
            }
            else
            {

                bool brake = tDistance < splineModeData.slowDownDistance;
                if (!brake)
                {
                    if (carBodyPhysic.getForwardVelocity() < _vehicleSetting.aiMaxSpeed)
                    {
                        AddForce(tf.normalized * splineModeData.accForce * 0.2f * splineModeData.accFactor);
                    }

                    Vector3 relativePos = _destination - carBodyPhysic.transform.position;
                    relativePos.y = 0;
                    FaceWaypoint(relativePos, splineModeData.turnSpeed, splineModeData.wheelSteeringFactor);
                }
                else
                {
                    if (carBodyPhysic.getForwardVelocity() > 0)
                    {
                        AddForce(-carBodyPhysic.transform.forward * splineModeData.decForce * 0.2f);
                    }

                    if (carBodyPhysic.getForwardVelocity() < 0)
                    {
                        carBodyPhysic.getRigidbody().velocity = Vector3.zero;
                    }
                }
            }
            
            // 模拟车轮表现
            carController.RollWheels(carBodyPhysic.getForwardVelocity() * splineModeData.wheelRotateSpeed);

            
            // 接音效
            float fv = carController.getSpeed() * 2.237f;
            float maxFv = _vehicleSetting.aiMaxSpeed;
            
            if (_audioComponent)
            {
                // 发动机音效
                float engineSoundValue = fv;
                // engineSoundValue *= 100;
                engineSoundValue = Mathf.Abs(engineSoundValue);
                _audioComponent.loopAdjustToneSound.SetSoundValue(engineSoundValue);
            }
        }

        
        
        public void AddForce(Vector3 velocity)
        {
            carBodyPhysic.getRigidbody().AddForce(velocity * Time.fixedDeltaTime / 0.034f,ForceMode.VelocityChange);
        }

        #endregion



        
        private bool curveDeceState = false;

        private void CalculateValues()
        {
            Vector3 targetVector = _destination - SteerPoint.position;
            Vector3 lastTargetVector = _targetVector;
            _targetVector = targetVector;

            float tDistance = 9999;
            
            float fv = carBodyPhysic.getForwardVelocity();
            float maxFv = carController.getWheelsMaxSpeed();

            if (_audioComponent)
            {
                // 发动机音效
                float engineSoundValue = fv / maxFv;
                engineSoundValue *= 100;
                engineSoundValue = Mathf.Abs(engineSoundValue);
                _audioComponent.loopAdjustToneSound.SetSoundValue(engineSoundValue);
            }
            
            carBodyFVelocity = fv;
            
            //UnityEngine.Debug.LogError(fv);
            carController.radiusBrake = 1;
            radiusBrake = 1;
            motorDecelerateFactor = 1f;

            carController.applyHandbrake = false;
            
            if (!curveDeceState)
            {
                motor2ZeroRadius = Mathf.Max(0,(fv - motorBrakeMinFv)) * motorRadiusFactor * motorRadiusCurve.Evaluate(fv/maxFv) + revertMotorRadius + 1;
            }

            curveDeceState = false;
            
            if (IsSpeedMode(VehicleSpeedStrategy.Const))
            {
                motorDecelerateFactor = 1f;

                carCaster.transform.position = SteerPoint.position - SteerPoint.forward * 1.5f;
                carCaster.transform.rotation = SteerPoint.rotation;
                
                carCaster.SetCastDistance(motor2ZeroRadius + 1.5f);
                carCaster.Cast();
                _hitInfos.Clear();
                carCaster.GetHitInfo(ref _hitInfos);
                if (_hitInfos.Count > 0)
                {
                    float minDis = 99999;
                    foreach (var hitInfo in _hitInfos)
                    {
                        if (hitInfo.collider == carController.CarCollider)
                        {
                            continue;
                        }
                        
                        float d = Vector3.Distance(SteerPoint.position, hitInfo.point);
                        if (d < minDis)
                        {
                            minDis = d;
                        }
                    }

                    tDistance = minDis;
                }
                SlowDown(tDistance, fv);
            }
            else if (IsSpeedMode(VehicleSpeedStrategy.Curve))
            {
                tDistance = targetVector.magnitude;
                SlowDown(tDistance, fv);
            }
            else if (IsSpeedMode(VehicleSpeedStrategy.Brake))
            {
                motorDecelerateFactor = 0;
                radiusBrake -= brakeSpeed * Time.fixedDeltaTime;
                radiusBrake =  Mathf.Max(0, carController.radiusBrake);
                carController.radiusBrake = radiusBrake;
            }
            
            //carController.getForwardVelocity()
        }

        private void Nav2Destination()
        {
            Vector3 targetVector = _destination - SteerPoint.position;
            targetVector.y = 0;

            Vector3 forward = SteerPoint.forward;
            forward.y = 0;
            
            if (steerStatus == VehicleSteerStatus.Ahead)
            {
                carController.setMotor(motorDeltaFactor * motorDecelerateFactor * externalMotorFactor);
            }
            else if (steerStatus == VehicleSteerStatus.Reverse)
            {
                carController.setMotor(-motorDeltaFactor * motorDecelerateFactor * externalMotorFactor);
            }

            float c = Vector3.Cross(forward, targetVector.normalized).y;

            float angle = 0f;
            if (steerStatus == VehicleSteerStatus.Ahead)
            {
                angle = Vector3.Angle(forward, targetVector.normalized);
            }
            else if (steerStatus == VehicleSteerStatus.Reverse)
            {
                 angle = Vector3.Angle(forward, targetVector.normalized);
            }

            float a = Mathf.Min(steeringAngleBound, angle);
            float s = steeringCurve.Evaluate(a / steeringAngleBound);
            
            if (c > 0)
            {
                // 右方
                carController.steeringDelta = ClampValue(angle * steeringFactor * s, -1, 1);
            }
            else
            {
                carController.steeringDelta = ClampValue(-angle * steeringFactor * s, -1, 1);
            }

        }

        private float ClampValue(float v, float min, float max)
        {
            return Mathf.Min(max, Mathf.Max(v, min));
        }
        
        private void SlowDown(float tDistance,float fv)
        { 
            if (tDistance < motor2ZeroRadius)
            {
                curveDeceState = true;
                if (tDistance >= revertMotorRadius)
                {
                    float absFv = Mathf.Abs(fv);
                    // 开始利用Motor减速
                    if (absFv > motorBrakeMinFv)
                    {
                        motorDecelerateFactor = 0;
                        // float t = (tDistance - revertMotorRadius)/ (motor2ZeroRadius - revertMotorRadius);
                        // t = motorBrakeCurve.Evaluate(t) * absFv/maxFv * motorBrakeFactor;
                        // motorDecelerateFactor = -t;
                    }
                    else
                    {
                        if (absFv > motorBrakeMinFv - 0.2f)
                        {
                            motorDecelerateFactor = 0;
                        }
                        else
                        {
                            motorDecelerateFactor = reveseMotorAccDelta;
                        }
                    }
                }
                else
                {
                    motorDecelerateFactor = 0;

                    if (IsSpeedMode(VehicleSpeedStrategy.HandBrake))
                    {
                        carController.applyHandbrake = true;

                        if (IsSpeedMode(VehicleSpeedStrategy.PreventBump))
                        {
                            if (tDistance < arriveRadius)
                            {
                                // 到了到达半径
                                carController.radiusBrake = 0;
                                radiusBrake = 0;
                            }
                        }
                    }
                    else if (IsSpeedMode(VehicleSpeedStrategy.ImmobilizeBrake))
                    {
                        // 容错，限制车辆和车轮的刚体角速度进行减速
                        if (tDistance < arriveRadius)
                        {
                            // 到了到达半径
                            carController.radiusBrake = 0;
                            radiusBrake = 0;
                        }
                        else
                        {
                            // 进入减速半径了
                            float t = (tDistance - arriveRadius)/ (revertMotorRadius - arriveRadius);
                            t = radiusBrakeCurve.Evaluate(t) * (1.0f/radiusBrakeFactor);
                            carController.radiusBrake = t;
                            radiusBrake = t;
                        }
                    }
                }
                
            }
            else
            {
                motorDecelerateFactor = 1f;
                carController.radiusBrake = 1f;
            }
        }
        
        private bool IsSpeedMode(VehicleSpeedStrategy mode)
        {
            return (speedStrategy & mode) > 0;
        }

        public void OnRevert()
        {
            if (carController)
            {
                carController.RollWheelsTo(-lastSteerAngle);
               
            }

            curveModeData.velocity = Vector3.zero;
            lastSteerAngle = 0;
            tDistance = 99999;
        }
        
#if UNITY_EDITOR

        private void OnDrawGizmos()
        {
            if (AIMode == VehicleAIMode.Physics)
            {
                if (SteerPoint!=null && Active)
                {
                    Gizmos.color = Color.cyan;
                    Gizmos.DrawWireSphere(SteerPoint.position,motor2ZeroRadius);
                    Gizmos.color = Color.green;
                    Gizmos.DrawWireSphere(SteerPoint.position,revertMotorRadius);
                    Gizmos.color = Color.red;
                    Gizmos.DrawWireSphere(SteerPoint.position,arriveRadius);
                }
            }

            if (Active && SteerPoint)
            {
                Gizmos.color = Color.green;
                Gizmos.DrawCube(_destination,new Vector3(0.5f,2f,0.5f));
                Gizmos.DrawLine(SteerPoint.position + new Vector3(0,0.3f,0),_destination + new Vector3(0,0.3f,0));
            }
            
        }

#endif
        
    }
}